#include "ODERobot.h"
#include "ODECommon.h"
#include "ODECustomMesh.h"
#include <ode/ode.h>
#include <math/angle.h>
#include <math3d/interpolate.h>
#include <Robotics/Rotation.h>
//#include "Dynamics/Misc.h"  //for AngVelToEulerAngles and EulerAnglesToAngVel

const static double kRobotCollisionPadding = 0.002;
const static double kRobotCoeffRestitution = 0.1;
const static double kRobotCoeffFriction = 1.0;

//given inertia matrix Hc about c, returns inertia matrix around origin
Matrix3 TranslateInertia(const Matrix3& Hc,const Vector3& c,Real mass)
{
  Matrix3 Ho=Hc;
  Ho(0,0) += mass*(c.y*c.y+c.z*c.z);
  Ho(1,1) += mass*(c.x*c.x+c.z*c.z);
  Ho(2,2) += mass*(c.y*c.y+c.x*c.x);
  Ho(0,1) -= mass*c.x*c.y;
  Ho(0,2) -= mass*c.x*c.z;
  Ho(1,2) -= mass*c.y*c.z;
  Ho(1,0) = Ho(0,1);
  Ho(2,0) = Ho(0,2);
  Ho(2,1) = Ho(1,2);
  return Ho;
}

ODERobot::ODERobot(Robot& _robot)
  :robot(_robot),jointGroupID(0),spaceID(0)
{
}

ODERobot::~ODERobot()
{
  Clear();
}

void ODERobot::Clear()
{
  for(size_t i=0;i<bodyID.size();i++) SafeDeleteProc(bodyID[i],dBodyDestroy);
  for(size_t i=0;i<jointID.size();i++) SafeDeleteProc(jointID[i],dJointDestroy);
  for(size_t i=0;i<geometry.size();i++) SafeDelete(geometry[i]);
  SafeDeleteProc(jointGroupID,dJointGroupDestroy);
  SafeDeleteProc(spaceID,dSpaceDestroy);

  bodyID.resize(0);
  geometry.resize(0);
  jointID.resize(0);
  jointFeedback.resize(0);
}

void ODERobot::Create(dWorldID worldID)
{
  Clear();

  bodyID.resize(robot.links.size(),NULL);
  geometry.resize(robot.links.size(),NULL);
  jointID.resize(robot.links.size(),NULL);
  jointFeedback.resize(robot.links.size());

  spaceID = dHashSpaceCreate(0);
  dHashSpaceSetLevels(spaceID,-3,0);
  dSpaceSetCleanup(spaceID,0);
  //dGeomSetData(spaceID,(void*)this);
  
  for(size_t i=0;i<robot.links.size();i++) {
    if(robot.links[i].mass != 0) {
      bodyID[i] = dBodyCreate(worldID);
      dMass mass;
      mass.mass = robot.links[i].mass;
      //NOTE: in ODE, COM must be zero vector!
      //CopyVector(mass.c,robot.links[i].com);
      mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1;
      CopyMatrix(mass.I,robot.links[i].inertia);
      if(robot.links[i].inertia.isZero()) {
	fprintf(stderr,"Warning, robot link %d has zero inertia, setting to identity * mass %g\n",i,robot.links[i].mass);
	Matrix3 temp; temp.setIdentity(); temp*=robot.links[i].mass;
	CopyMatrix(mass.I,temp);
      }
      int res=dMassCheck(&mass);
      if(res != 1) {
	fprintf(stderr,"Uh... mass of link %d is not considered to be valid by ODE?\n",i);
	std::cerr<<"Inertia: "<<robot.links[i].inertia<<std::endl;
      }
      dBodySetMass(bodyID[i],&mass);
      
      if(!robot.geometry[i].tris.empty()) {
	geometry[i] = new ODETriMesh;
	geometry[i]->Create(robot.geometry[i],spaceID,-robot.links[i].com);
	dGeomSetBody(geometry[i]->geom(),bodyID[i]);
	dGeomSetData(geometry[i]->geom(),(void*)i);
	geometry[i]->SetPadding(kRobotCollisionPadding);
	geometry[i]->surf().kRestitution = kRobotCoeffRestitution;
	geometry[i]->surf().kFriction = kRobotCoeffFriction;
	geometry[i]->surf().kStiffness = Inf;
	geometry[i]->surf().kDamping = Inf;
      }
    }
  }
  Config oldQ = robot.q;
  robot.q.setZero();
  robot.UpdateFrames();
  SetConfig(robot.q);

  Assert(jointGroupID == 0);
  jointGroupID = dJointGroupCreate(0);
  for(size_t i=0;i<robot.joints.size();i++) {
    switch(robot.joints[i].type) {
    case RobotJoint::Weld:
      {
	int link = robot.joints[i].linkIndex;
	int parent = robot.parents[link];
	dBodyID bp = (parent == -1? 0 : bodyID[parent]);
	jointID[link] = dJointCreateFixed(worldID,jointGroupID);
	dJointAttach(jointID[link],bodyID[link],bp);
	dJointSetFeedback(jointID[link],&jointFeedback[link]);
	Vector3 anchor = robot.links[link].T_World*robot.links[link].com;
	dJointSetFixed(jointID[link]);
      }
      break;
    case RobotJoint::Normal:
      {
	int link = robot.joints[i].linkIndex;
	int parent = robot.parents[link];
	dBodyID bp = (parent == -1? 0 : bodyID[parent]);
	if(robot.links[link].type == RobotLink3D::Revolute) {
	  jointID[link] = dJointCreateHinge(worldID,jointGroupID);
	  dJointAttach(jointID[link],bodyID[link],bp);
	  Vector3 pos = robot.links[link].T_World.t;
	  Vector3 axis = robot.links[link].T_World.R*robot.links[link].w;
	  dJointSetHingeAnchor(jointID[link],pos.x,pos.y,pos.z);
	  dJointSetHingeAxis(jointID[link],axis.x,axis.y,axis.z);
	  //stops are currently not working correctly -- must be within [-pi,pi]
	  //dJointSetHingeParam(jointID[link],dParamLoStop,robot.qMin(link)+1e-3);
	  //dJointSetHingeParam(jointID[link],dParamHiStop,robot.qMax(link)-1e-3);
	  dJointSetHingeParam(jointID[link],dParamBounce,0);
	  dJointSetHingeParam(jointID[link],dParamFMax,0);
	}
	else {
	  jointID[link] = dJointCreateSlider(worldID,jointGroupID);
	  dJointAttach(jointID[link],bodyID[link],bp);
	  Vector3 axis = robot.links[link].T_World.R*robot.links[link].w;
	  dJointSetSliderAxis(jointID[link],axis.x,axis.y,axis.z);
	  //stops are currently not working correctly -- must be within [-pi,pi]
	  //dJointSetSliderParam(jointID[link],dParamLoStop,robot.qMin(link)+1e-3);
	  //dJointSetSliderParam(jointID[link],dParamHiStop,robot.qMax(link)-1e-3);
	  dJointSetSliderParam(jointID[link],dParamBounce,0);
	  dJointSetSliderParam(jointID[link],dParamFMax,0);
	}
	dJointSetFeedback(jointID[link],&jointFeedback[link]);
      }
      break;
    case RobotJoint::Floating:
      break;
    default:
      FatalError("TODO: setup affine and other custom joints\n");
      break;
    }
  }
  robot.UpdateConfig(oldQ);
  SetConfig(oldQ);
  UpdateLastTransforms();
}

void ODERobot::SetJointDryFriction(int joint,Real coeff)
{
  int k=robot.joints[joint].linkIndex;
  SetLinkDryFriction(k,coeff);
}

void ODERobot::SetLinkDryFriction(int k,Real coeff)
{
  if(!jointID[k]) return;
  if(robot.links[k].type == RobotLink3D::Revolute) {
    dJointSetHingeParam(jointID[k],dParamVel,0);
    dJointSetHingeParam(jointID[k],dParamFMax,coeff);
  }
  else {
    dJointSetSliderParam(jointID[k],dParamVel,0);
    dJointSetSliderParam(jointID[k],dParamFMax,coeff);
  }
}

void ODERobot::SetJointFixedVelocity(int joint,Real vel,Real tmax)
{
  SetLinkFixedVelocity(robot.joints[joint].linkIndex,vel,tmax);
}

void ODERobot::SetLinkFixedVelocity(int k,Real vel,Real tmax)
{
  if(!jointID[k]) return;
  if(robot.links[k].type == RobotLink3D::Revolute) {
    dJointSetHingeParam(jointID[k],dParamVel,vel);
    dJointSetHingeParam(jointID[k],dParamFMax,tmax);
  }
  else {
    dJointSetSliderParam(jointID[k],dParamVel,vel);
    dJointSetSliderParam(jointID[k],dParamFMax,tmax);
  }
}

void ODERobot::SetConfig(const Config& q)
{
  Assert(q == robot.q);
  for(size_t i=0;i<robot.links.size();i++) {
    SetBodyTransform(i,robot.links[i].T_World);
  }
}

void ODERobot::GetConfig(Config& q) const
{
  q.resize(robot.links.size());
  for(size_t i=0;i<robot.joints.size();i++) {
    int k=robot.joints[i].linkIndex;
    switch(robot.joints[i].type) {
    case RobotJoint::Weld:
      q(k)=robot.q(k);
      break;
    case RobotJoint::Normal:
      if(robot.links[k].type == RobotLink3D::Revolute) 
	q(k)=dJointGetHingeAngle(jointID[k]);
      else
	q(k)=dJointGetSliderPosition(jointID[k]);
      break;
    case RobotJoint::Floating:
      {
	//first, get the root position
	RigidTransform T;
	GetBodyTransform(k,T);
	robot.SetJointByTransform(i,k,T);
	vector<int> indices;
	robot.GetJointIndices(i,indices);
	for(size_t j=0;j<indices.size();j++)
	  q(indices[j]) = robot.q(indices[j]);
	break;
      }
    default:
      FatalError("TODO");
      break;
    }
  }
  robot.NormalizeAngles(q);
  //cout<<"q = "<<q<<endl;
}

Real ODERobot::GetJointAngle(int joint) const
{
  return GetLinkAngle(robot.joints[joint].linkIndex);
}

Real ODERobot::GetLinkAngle(int i) const
{
  if(!jointID[i]) return 0;
  if(robot.links[i].type == RobotLink3D::Revolute) {
    Real val=dJointGetHingeAngle(jointID[i]);
    //normalize angle
    if(robot.links[i].type == RobotLink3D::Revolute) {
      Real qi = AngleNormalize(val);
      if(qi > robot.qMax(i)) {
	if(qi - TwoPi >= robot.qMin(i)) 
	  qi -= TwoPi;
	else if(Abs(qi - TwoPi - robot.qMin(i)) < Abs(qi - robot.qMax(i)))
	  qi -= TwoPi;
      }
      if(qi < robot.qMin(i)) {
	if(qi + TwoPi <= robot.qMax(i))
	  qi += TwoPi;
	else if(Abs(qi + TwoPi - robot.qMax(i)) < Abs(qi - robot.qMin(i)))
	  qi += TwoPi;
      }
      val = qi;
    }
    return val;
  }
  else {
    return dJointGetSliderPosition(jointID[i]);
  }
}

void ODERobot::SetBodyTransform(int body,const RigidTransform& T)
{
  if(!bodyID[body]) return;
  Vector3 comPos = T*robot.links[body].com;
  dBodySetPosition(bodyID[body],comPos.x,comPos.y,comPos.z);
  dMatrix3 rot;
  CopyMatrix(rot,T.R);
  dBodySetRotation(bodyID[body],rot);
}

void ODERobot::GetBodyTransform(int body,RigidTransform& T) const
{
  if(!bodyID[body]) {
    T.setIdentity();
    return;
  }
  const dReal* pos = dBodyGetPosition(bodyID[body]);
  Vector3 comPos(pos[0],pos[1],pos[2]);
  const dReal* rot = dBodyGetRotation(bodyID[body]);
  CopyMatrix(T.R,rot);
  T.t = comPos - T.R*robot.links[body].com;
}

void ODERobot::SetBodyVelocity(int body,const Vector3& w,const Vector3& v)
{
  dBodySetLinearVel(bodyID[body],v.x,v.y,v.z);
  dBodySetAngularVel(bodyID[body],w.x,w.y,w.z);
}

void ODERobot::GetBodyVelocity(int body,Vector3& w,Vector3& v) const
{
  if(!bodyID[body]) {
    v.setZero();
    w.setZero();
    return;
  }
  CopyVector(v,dBodyGetLinearVel(bodyID[body]));
  CopyVector(w,dBodyGetAngularVel(bodyID[body]));
}

void ODERobot::SetVelocities(const Config& dq)
{
  Vector3 zero(Zero);
  Vector3 v,w;
  for(size_t i=0;i<robot.links.size();i++) {
    if(!bodyID[i]) continue;
    robot.GetWorldVelocity(robot.links[i].com,i,dq,v);
    robot.GetWorldAngularVelocity(i,dq,w);
    dBodySetLinearVel(bodyID[i],v.x,v.y,v.z);
    dBodySetAngularVel(bodyID[i],w.x,w.y,w.z);
  }

  //DEBUG
  Vector temp=dq;
  GetVelocities(temp);
  if(!temp.isEqual(dq,1e-4)) {
    cout<<"ODERobot::SetVelocities: Error, Get/SetVelocities don't match"<<endl;
    cout<<"dq = "<<dq<<endl;
    cout<<"from GetVelocities = "<<temp<<endl;
  }
  Assert(temp.isEqual(dq,1e-4));
}

void ODERobot::GetVelocities(Config& dq) const
{
  dq.resize(robot.links.size(),Zero);
  //get dq for the joints
  for(size_t i=0;i<robot.joints.size();i++) {
    int k=robot.joints[i].linkIndex;
    switch(robot.joints[i].type) {
    case RobotJoint::Weld:
      break;
    case RobotJoint::Normal:
      if(robot.links[k].type == RobotLink3D::Revolute) 
	dq(k)=dJointGetHingeAngleRate(jointID[i]);
      else
	dq(k)=dJointGetSliderPositionRate(jointID[i]);
      break;
    case RobotJoint::Floating: 
      {
	vector<int> indices;
	robot.GetJointIndices(i,indices);
	Assert(indices.size()==6 || indices.size()==3);
	Assert(indices.back()==k);
	//get dq for the root link
	RigidTransform T;
	Vector3 w,v;
	GetBodyVelocity(k,w,v);
	GetBodyTransform(k,T);
	//v = x' + w x (R*comloc)
	Vector3 comloc = T.R*robot.links[k].com;
	Vector3 dx = v-cross(w,comloc);
	robot.SetJointVelocityByMoment(i,k,w,dx);
	for(size_t j=0;j<indices.size();j++)
	  dq(indices[j]) = robot.dq(indices[j]);
      }
      break;
    default:
      FatalError("TODO");
      break;
    }
  }
}

Real ODERobot::GetJointVelocity(int joint) const
{
  return GetLinkVelocity(robot.joints[joint].linkIndex);
}

Real ODERobot::GetLinkVelocity(int i) const
{
  if(!jointID[i]) return 0;
  if(robot.links[i].type == RobotLink3D::Revolute) 
    return dJointGetHingeAngleRate(jointID[i]);
  else
    return dJointGetSliderPositionRate(jointID[i]);
}

void ODERobot::AddTorques(const Config& t)
{
  Assert(t.n == (int)robot.links.size());
  for(int i=0;i<t.n;i++)
    if(!IsFinite(t(i))) {
      printf("Error, commanding link %d to a non-finite torque!\n",i);
      getchar();
      return;
    }

  /*
  EulerAnglesToAngVel(theta,Vector3(t(3),t(4),t(5)),m);
  dBodyAddTorque(bodyID[0],m.x,m.y,m.z);
  //additional force due to COM shift
  Vector3 f2 = cross(m,Rcm);
  //Matrix3 inertiaInv; inertiaInv.setInverse(robot.links[5].inertia);
  //Vector3 f2=cross(inertiaInv*m,T.R*robot.links[5].com)*robot.links[5].mass;
  dBodyAddForce(bodyID[0],f2.x,f2.y,f2.z);
  */

  for(size_t i=0;i<robot.joints.size();i++) {
    int k=robot.joints[i].linkIndex;
    switch(robot.joints[i].type) {
    case RobotJoint::Weld:
      break;
    case RobotJoint::Normal:
      if(robot.links[k].type == RobotLink3D::Revolute) 
	dJointAddHingeTorque(jointID[k],t(k));
      else
	dJointAddSliderForce(jointID[k],t(k));
      break;
    case RobotJoint::Floating:
      {
	//for floating, convert generalized torques to true force/moment
	//Jt (f,m) = t
	//J = [dcm/dq] = [I A]*[t[1-3]]
	//    [dR/dq ]   [0 B] [t[4-6]]
	//A = -[Rcm]B
	//f = t[1-3]
	//m = B^-t t[4-6] + t[1-3] x Rcm
	//TODO: generalize to anything besides RPY rotations
	int tx,ty,tz;
	int rx,ry,rz;
	vector<int> indices;
	robot.GetJointIndices(i,indices);
	Assert(indices.size()==6 || indices.size()==3);
	if(indices.size()==6) {
	  dBodyAddForce(bodyID[k],t(tx),t(ty),t(tz));
	  RigidTransform T;
	  GetBodyTransform(k,T);
	  EulerAngleRotation theta;
	  theta.setMatrixZYX(T.R);
	  Vector3 m;
	  Vector3 Rcm = T.R*robot.links[k].com;
	  Matrix3 Bt,Btinv;
	  EulerAngleMoments(theta,2,1,0,Bt);
	  Bt.inplaceTranspose();
	  Btinv.setInverse(Bt);
	  //Btinv = Bt;
	  Btinv.mul(Vector3(t(rx),t(ry),t(rz)),m);
	  m += cross(Vector3(t(tx),t(ty),t(tz)),Rcm);
	  dBodyAddTorque(bodyID[k],m.x,m.y,m.z);
	}
	else {
	  dBodyAddForce(bodyID[k],t(tx),t(ty),t(tz));
	}
      }
      break;
    default:
      FatalError("TODO");
    }
  }
}

void ODERobot::AddJointTorque(int joint,Real t)
{
  AddLinkTorque(robot.joints[joint].linkIndex,t);
}

void ODERobot::AddLinkTorque(int i,Real t)
{
  if(!jointID[i]) { 
    printf("ODERobot::AddLinkTorque: Warning, no joint %d\n",i);
    return;
  }
  if(!IsFinite(t)) {
    printf("ODERobot::AddLinkTorque: Error, commanding link %d to a non-finite torque!\n",i);
    getchar();
    return;
  }
  if(robot.links[i].type == RobotLink3D::Revolute)   
    dJointAddHingeTorque(jointID[i],t);
  else
    dJointAddSliderForce(jointID[i],t);
}

Real ODERobot::GetDriverValue(int driver) const
{
  const RobotJointDriver& d=robot.drivers[driver];
  switch(d.type) {
  case RobotJointDriver::Normal:
    return GetLinkAngle(d.linkIndices[0]);
  case RobotJointDriver::Translation:
    {
      RigidTransform T;
      GetBodyTransform(d.linkIndices[1],T);
      return robot.links[d.linkIndices[0]].w.dot(T.t);
    }
    break;
  case RobotJointDriver::Rotation:
    {
      RigidTransform T;
      GetBodyTransform(d.linkIndices[1],T);
      FatalError("What to do with rotation?");
    }
    break;
  case RobotJointDriver::Affine: 
    {
      Real vavg=0;
      for(size_t i=0;i<d.linkIndices.size();i++)
	vavg += (GetLinkAngle(d.linkIndices[i])-d.affOffset[i])/d.affScaling[i];
      return vavg / d.linkIndices.size();
    }
    break;
  default:
    FatalError("TODO");
    break;
  }
}

Real ODERobot::GetDriverVelocity(int driver) const
{
  const RobotJointDriver& d=robot.drivers[driver];
  switch(d.type) {
  case RobotJointDriver::Normal:
    return GetLinkVelocity(d.linkIndices[0]);
  case RobotJointDriver::Translation:
    {
      Vector3 w,v;
      GetBodyVelocity(d.linkIndices[1],w,v);
      return robot.links[d.linkIndices[0]].w.dot(v);
    }
    break;
  case RobotJointDriver::Rotation:
    {
      Vector3 w,v;
      GetBodyVelocity(d.linkIndices[1],w,v);
      return robot.links[d.linkIndices[0]].w.dot(w);
    }
    break;
  case RobotJointDriver::Affine: 
    {
      Real vavg=0;
      for(size_t i=0;i<d.linkIndices.size();i++)
	vavg += GetLinkVelocity(d.linkIndices[i])/d.affScaling[i];
      return vavg / d.linkIndices.size();
    }
    break;
  default:
    FatalError("TODO");
    break;
  }
}

void ODERobot::AddDriverTorque(int driver,Real t)
{
  const RobotJointDriver& d=robot.drivers[driver];
  switch(d.type) {
  case RobotJointDriver::Normal:
    AddLinkTorque(d.linkIndices[0],t);
    break;
  case RobotJointDriver::Translation:
    {
      Assert(bodyID[d.linkIndices[1]] != NULL);
      Vector3 f=robot.links[d.linkIndices[0]].w*t;
      dBodyAddForce(bodyID[d.linkIndices[1]],f.x,f.y,f.z);
    }
    break;
  case RobotJointDriver::Rotation:
    {
      Assert(bodyID[d.linkIndices[1]] != NULL);
      Vector3 m=robot.links[d.linkIndices[0]].w*t;
      dBodyAddTorque(bodyID[d.linkIndices[1]],m.x,m.y,m.z);
    }
    break;
  case RobotJointDriver::Affine:
    {
      for(size_t i=0;i<d.linkIndices.size();i++)
	AddLinkTorque(d.linkIndices[i],t*d.affScaling[i]);
    }
    break;
  default:
    FatalError("TODO");
  }
}

void ODERobot::SetDriverFixedVelocity(int driver,Real vel,Real tmax)
{
  const RobotJointDriver& d=robot.drivers[driver];
  switch(d.type) {
  case RobotJointDriver::Normal:
    SetLinkFixedVelocity(d.linkIndices[0],vel,tmax);
    break;
  case RobotJointDriver::Affine:
    for(size_t i=0;i<d.linkIndices.size();i++) {
      SetLinkFixedVelocity(d.linkIndices[i],vel*d.affScaling[i],tmax);
    }
    break;
  default:
    FatalError("TODO");
  }
}

void ODERobot::AddDriverTorques(const Config& t)
{
  Assert(t.n == (int)robot.drivers.size());
  for(int i=0;i<t.n;i++)
    AddDriverTorque(i,t(i));
}

void ODERobot::UpdateLastTransforms()
{
  for(size_t i=0;i<bodyID.size();i++) {
    if(!bodyID[i]) continue;
    if(!geometry[i]) continue;
    RigidTransform T;
    const dReal* pos = dBodyGetPosition(bodyID[i]);
    const dReal* rot = dBodyGetRotation(bodyID[i]);
    CopyVector(T.t,pos);
    CopyMatrix(T.R,rot);
    Matrix4 mat(T);
    geometry[i]->SetLastTransform(mat);
  }
}

bool ODERobot::ReadState(File& f)
{
  int initPos = f.Position();
  for(size_t i=0;i<robot.links.size();i++) {
    if(bodyID[i] == NULL)  continue;
    RigidTransform Told;
    Vector3 w,v;
    dReal pos[3];
    dReal q[4];
    if(!ReadArrayFile(f,pos,3)) return false;
    if(!ReadArrayFile(f,q,4)) return false;
    if(!ReadFile(f,w)) return false;
    if(!ReadFile(f,v)) return false;
    if(!ReadFile(f,Told.R)) return false;
    if(!ReadFile(f,Told.t)) return false;

    dBodySetPosition(bodyID[i],pos[0],pos[1],pos[2]);
    dBodySetQuaternion(bodyID[i],q);
    SetBodyVelocity(i,w,v);
    Matrix4 mat(Told);
    if(geometry[i])
      geometry[i]->SetLastTransform(mat);
  }

  /*
  File f2;
  f2.OpenData();
  WriteState(f2);
  void* d1=f.GetDataBuffer();
  void* d2=f2.GetDataBuffer();
  int l1=f.Position()-initPos;
  int l2=f2.Position();
  if(l1 != l2) {
    FatalError("ODERobot::Write/ReadState() do not read the same number of bits %d vs %d!",l1,l2);
  }
  */
  /*
  if(memcmp(((char*)d1)+initPos,d2,l1) != 0) {
    FatalError("ODERobot::Write/ReadState() do not work correctly!");
  }
  */
  return true;
}

bool ODERobot::WriteState(File& f) const
{
  for(size_t i=0;i<robot.links.size();i++) {
    if(bodyID[i] == NULL)  continue;
    RigidTransform Told;
    Vector3 w,v;

    const dReal* pos=dBodyGetPosition(bodyID[i]);
    const dReal* q=dBodyGetQuaternion(bodyID[i]);
    GetBodyVelocity(i,w,v);
    Matrix4 mat;
    if(geometry[i])
      geometry[i]->GetLastTransform(mat);
    else
      mat.setIdentity();
    Assert(mat(3,0)==0);
    Assert(mat(3,1)==0);
    Assert(mat(3,2)==0);
    Told.set(mat);

    if(!WriteArrayFile(f,pos,3)) return false;
    if(!WriteArrayFile(f,q,4)) return false;
    if(!WriteFile(f,w)) return false;
    if(!WriteFile(f,v)) return false;
    if(!WriteFile(f,Told.R)) return false;
    if(!WriteFile(f,Told.t)) return false;
  }
  return true;
}

/*
void ODERobot::InterpolateState(const Vector& x,const Vector& y,Real u)
{
  Assert(x.n == (int)robot.links.size()*30);   //position, rotation, velocity, ang velocity, last transform
  Assert(y.n == (int)robot.links.size()*30);   //position, rotation, velocity, ang velocity, last transform

  int k=0;
  for(size_t i=0;i<robot.links.size();i++) {
    RigidTransform Tx,Txold,Ty,Tyold;
    Vector3 wx,vx,wy,vy;
    for(int p=0;p<3;p++)
      for(int q=0;q<3;q++) {
	Tx.R(p,q)=x(k+p*3+q);
	Ty.R(p,q)=y(k+p*3+q);
      }
    k += 9;
    Tx.t.set(x(k),x(k+1),x(k+2));
    Ty.t.set(y(k),y(k+1),y(k+2));
    k+=3;
    wx.set(x(k),x(k+1),x(k+2));
    wy.set(y(k),y(k+1),y(k+2));
    k+=3;
    vx.set(x(k),x(k+1),x(k+2));
    vy.set(y(k),y(k+1),y(k+2));
    k+=3;
    for(int p=0;p<3;p++)
      for(int q=0;q<3;q++) {
	Txold.R(p,q)=x(k+p*3+q);
	Tyold.R(p,q)=y(k+p*3+q);
      }
    k += 9;
    Txold.t.set(x(k),x(k+1),x(k+2));
    Tyold.t.set(y(k),y(k+1),y(k+2));
    k+=3;

    RigidTransform T,Told;
    Vector3 w,v;
    interpolate(Tx,Ty,u,T);
    interpolate(wx,wy,u,w);
    interpolate(vx,vy,u,v);
    interpolate(Txold,Tyold,u,Told);
    for(int p=0;p<3;p++)
      for(int q=0;q<3;q++) 
	Assert(IsFinite(T.R(p,q)));
    for(int p=0;p<3;p++)
      for(int q=0;q<3;q++) 
	Assert(IsFinite(Told.R(p,q)));
    for(int p=0;p<3;p++) {
      Assert(IsFinite(T.t[p]));
      Assert(IsFinite(Told.t[p]));
      Assert(IsFinite(w[p]));
      Assert(IsFinite(v[p]));
    }

    SetBodyTransform(i,T);
    SetBodyVelocity(i,w,v);
    Matrix4 mat(Told);
    geometry[i]->SetLastTransform(mat);
  }
  Assert(k == x.n);
}
*/
